#include "deal_datapacket.h"

FLY_TYPE FLY;

u8 SENSER_OFFSET_FLAG;

u8 packetData[12];		//打包后待发送的数据包，这个长度必须和接收端SI24R1的接收通道定义的有效数据长度相同，否则接收不到数据
u8 dataPID = 0;			//数据包识别PID	
vu16 accelerator = 0;	//记录油门上一次的数值，用于下拉油门的时候，防止动力损失过大造成的失衡

//extern u8 RxBuf[RX_PLOAD_WIDTH];	
//extern vu8 ButtonMask;				//用来保存哪个按键刚被按下了，原始定义在：button.c文件中

vu16 ADC_Value[4];

/**
 * @brief ADC校准
 * @param None
 * @retval None
 * @note 校正步骤：1、拉低油门，右手柄移到左下角开始校正，2、再
次移动有手柄到居中位置（如果右手臂是上下左右自动回中的，完成第一步
后主动松手就行了
 **/
void ADC_Calibration(void)
{
	static u8 cnt = 0;
	static vs32 temp1 = 0, temp2 = 0, temp3 = 0, temp4 = 0;
	if (ADC_ConvertedValue[2] < 6 && ADC_ConvertedValue[3] < 6 && ADC_ConvertedValue[1] < 6 && ADC_CALIBRATOR_OVER == 0)
	{
		ADC_CALIBRATOR_FLAG = 1;
		ADC_CALIBRATOR_OVER = 1;
	
	}
	if (ADC_CALIBRATOR_FLAG)
	{
		if (ADC_ConvertedValue[2] > 1900 && ADC_ConvertedValue[2] < 2196 && ADC_ConvertedValue[3] > 1900 && ADC_ConvertedValue[3] < 2196)
		{
			if (0 == cnt)
			{
				temp1 = 0;
				temp2 = 0;
				temp3 = 0;
				temp4 = 0;
				cnt = 0;
			}
			temp1 += (1000 + ADC_ConvertedValue[0] * 1000 / 4096);
			temp2 += (1000 + ADC_ConvertedValue[1] * 500 / 4096);
			temp3 += (1000 + ADC_ConvertedValue[2] * 1000 / 4096);
			temp4 += (1000 + ADC_ConvertedValue[3] * 1000 / 4096);
			cnt++;
			if (cnt >= 60)
			{
				ADC_Calibrator[0] = temp1 / cnt;
				ADC_Calibrator[1] = temp2 / cnt;
				ADC_Calibrator[2] = temp3 / cnt;
				ADC_Calibrator[3] = temp4 / cnt;
				ADC_CALIBRATOR_FLAG = 0;
				cnt = 0;
				//	PID_WriteFlash();
			}
		}
	}
}

/**
 * @brief ADC值限幅
 * @param value 待限幅ADC值
 * @param L 左偏差
 * @param R 右偏差
 * @param min 最小值
 * @param mac 最大值
 * @retval 限幅后的值
 **/
vu16 ADC_ValueLimit(vu16 value,vu16 L,vu16 R,vu16 min,vu16 max)
{
	vu16 result = 0;
	if(value>L&&value<R) 
		value= 1500;
	if(value < L)
		value += 1500-L;
	if(value > R)
		value -= 1500-L;
	
	value = (value<=min)?min:value;//防止越界
	value = (value>=max)?max:value;//防止越界

	result = value;
	return result;
}

void PackData(u8 firstByte)
{	
	ADC_Value[0] = 1500 + ((1000+ADC_ConvertedValue[0]*1000/4096) - ADC_Calibrator[0]);
	ADC_Value[0] = ADC_ValueLimit(ADC_Value[0],1470,1530,1100,1900);

	ADC_Value[1] = (ADC_ConvertedValue[1]*1000/4096); //油门0~1000;
	
	ADC_Value[2] = 1500 + ((1000+(ADC_ConvertedValue[2])*1000/4096) - ADC_Calibrator[2] );
	ADC_Value[2] = ADC_ValueLimit(ADC_Value[2],1470,1530,1100,1900);
	
	ADC_Value[3] = 1500 + ((1000+(ADC_ConvertedValue[3])*1000/4096) - ADC_Calibrator[3] );
	ADC_Value[3] = ADC_ValueLimit(ADC_Value[3],1470,1530,1100,1900);
	
	//数据包识别PID自增，并且超过200时自动归零
	if(dataPID>=200){
		dataPID = 0;
	}else{
		dataPID++;
	}
	
//=========直接采用指针操作内存中的数值将16位转成8位，速度快，并且不会发生精度截取的现象，还要注意，STM32是小端地址============
	packetData[0] = firstByte;					  //前导码
//	packetData[1] = ButtonMask;                   //按键
	packetData[2] = *(((u8*)ADC_Value)+0);        //YAW角
	packetData[3] = *(((u8*)ADC_Value)+1);
	packetData[4] = *(((u8*)ADC_Value)+2);		  //油门数据打包
	packetData[5] = *(((u8*)ADC_Value)+3);
	packetData[6] = *(((u8*)ADC_Value)+4);        //ROLL角
	packetData[7] = *(((u8*)ADC_Value)+5);
	packetData[8] = *(((u8*)ADC_Value)+6);        //PITCH角
	packetData[9] = *(((u8*)ADC_Value)+7);
	packetData[10] = dataPID;					  //这个非常重要，这是防止飞机逃脱遥控的保证
	packetData[11] = 0xa5;						  //校验码：1010 0101
}

void DataDeal_task(void *pvParameters)
{
	ADC_SoftwareStartConvCmd(ADC1, ENABLE);	  //使能指定的ADC1的软件转换启动功能
	while(1)
	{
		if(ulTaskNotifyTake(pdFALSE,portMAX_DELAY))//阻塞等待ADC转换完成
		{
			DEBUG("Start deal data!\n");
			ADC_Calibration();//ADC校正
			//PackData(0x01);   //数据打包，帧头为0x01
			ADC_Cmd(ADC1, ENABLE );
            ADC_SoftwareStartConvCmd(ADC1, ENABLE);
		}
	}
}

